// Run this Program in ESP Lionlite  to control BLDC driver through UART1. BLDC Drvier ESP32 should uploaded with full control firmware
//  Setup:
//  1) Connect ESP Lionlite UART (TX1) to BLDC UART1(RX) 2) ESP Lionlite connect to PC 3) Send motor cmd eg" T 10" to serial port 4)BLDC motor should move
// Enable ESP32 wifi to connect with Ubuntu
// Pub and receive message through WIFI

#include <Arduino.h>
#include <Wire.h>
// #include <servo.h>
// ESP32Servo Lib
// #include <Servo.h>
#include <motor.h>
#include <main.h>
// #include <blink_ble.h>
#include <SimpleFOC.h>

//Enable ESP32 wifi
#include <WiFi.h>
const char* ssid = "CMCC-Athlon";
const char* password =  "haha1234";

//include ArduinoJson lib for encode/decode json message
#include <ArduinoJson.h>

//Enable Arudion Web client lib, connect to PC Ubuntu ROS by WIFI
#include <ArduinoWebsockets.h>
const char* websocket_server_host = "192.168.1.10";
const uint16_t websocket_server_port = 8080;


using namespace websockets;
WebsocketsClient client;

//set default speed to 0 when no publish from ROS
float linear_x = 0;
float angular_z = 0;


//decode jason message from Ubuntu ROS server
void onMessageCallback(WebsocketsMessage message) {
  // 尝试解析JSON消息
  DynamicJsonDocument doc(1024);
  DeserializationError error = deserializeJson(doc, message.data());
  

  if (error) {
    Serial.print("deserializeJson() failed: ");
    Serial.println(error.c_str());
    return;
  }

  // 检查是否为/cmd_vel消息
  if(doc["topic"].as<String>() == "/cmd_vel") {
    linear_x = doc["msg"]["linear"]["x"]; // 获取线性速度x
    angular_z = doc["msg"]["angular"]["z"]; // 获取角速度z

    Serial.print("线性速度 X: ");
    Serial.print(linear_x);
    Serial.print("，角速度 Z: ");
    Serial.println(angular_z);

    // 在这里使用线性速度和角速度值
  }
}

void onEventsCallback(WebsocketsEvent event, String data) {
  if(event == WebsocketsEvent::ConnectionOpened) {
    Serial.println("Connnection Opened");
    // 订阅/cmd_vel话题
    client.send("{\"op\": \"subscribe\", \"topic\": \"/cmd_vel\"}");
  } else if(event == WebsocketsEvent::ConnectionClosed) {
    Serial.println("Connnection Closed");
  }
}

// end of decode, athlon


void setup()
{
  // put your setup code here, to run once:
  Serial.begin(9600);
  // Setup BLDC Driver UART ports
  //V1 Control Board
  //Serial1.begin(115200, SERIAL_8N1, 0, 2);   // Serial_motor_left, uart1 in schematic，close to corner in layout
  //Serial2.begin(115200, SERIAL_8N1, 15, 13); // Serial_motor_right, uart0 in schematic, RX FIRST THEN TX
  
  //For V2 Control Board, serial1 test working,swap motor control serial port solve  speed diff issue,why??? 
  //PS: type C接口有方向，有一面无法识别！！！
  //电源线接触问题会导致motor serial 没有输出，可以通过扭矩判断，没有扭矩就是没有上电
  // Motor driver set to 9600 to avoid transfer error
  Serial1.begin(9600, SERIAL_8N1, 13, 15);   // Serial_motor_left, uart1 in schematic，close to corner in layout
  Serial2.begin(9600, SERIAL_8N1, 16, 17); // Serial_motor_right, uart0 in schematic, RX FIRST THEN TX

 // Serial.print("Controller Program is running... : ");

  // Servo attach
  //  for(int i = 0; i < 2; ++i) {
  //        if(!servos[i].attach(servosPins[i])) {
  //            Serial.print("Servo ");
  //            Serial.print(i);
  //            Serial.println("attach error");
  //        }
  //    }

  //stop motor when mpu calibration
 // set_motor_speed(0,0);   //  1st is right hand wheel, swap motor uart port solve wheel speed diff problem，why??? 


  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(500);
    Serial.println("连接WiFi...");
  }

  Serial.println("已连接WiFi");

  // 连接到WebSocket服务器
  client.connect(websocket_server_host, websocket_server_port, "/");
  if (client.available()) {
   Serial.println("已连接到rosbridge服务器");
  }

  // 连接到WebSocket服务器
  client.onMessage(onMessageCallback);
  client.onEvent(onEventsCallback);
  //client.connect(websocket_server);
  // if (client.available()) {
  //   Serial.println("已连接到rosbridge服务器");
  // }

}




void loop()
{
  // put your main code here, to run repeatedly:
  // Get MPU6050 data
  

//remove comments here; left motor speed is 12 less than right in 6V mode， why???
 read_left_motor(); // read_left_motor_speed  angel,serial 
 read_right_motor(); // read_right_motor_speed  angel, serial 2 
////////////////////
 
  Serial.print("left: ");
  Serial.print(left_motor_speed);
  Serial.print("\tright: ");
  Serial.println(right_motor_speed);
  //delay(500);

   
  // Serial.print("--> control voltage : ");
  // Serial.println(control_voltage);

  
  // 连接到WebSocket服务器, add below into loop
  client.onMessage(onMessageCallback);
  client.onEvent(onEventsCallback);

  client.connect(websocket_server_host, websocket_server_port, "/");
  if (client.available()) {
    Serial.println("已连接到rosbridge服务器");
  }

//talk to Ubuntu ROS here
  if(client.available()) {
  client.poll();
  }

  // 这里可以根据需要添加代码，例如周期性地发布当前速度
  set_motor_speed(linear_x);
  delay(1000);


  // //send "T X " cmd to motor through serial port 1/2, actually it's voltage setting 
  // in Torch control mode, in speed control mode, it is speed setting
  //  set_motor_speed(-3,3);
  //  delay(5000);
    
    // set same 6,6 voltage. left speed is -4.5, right is 19 ?? Why?
  // set_motor_speed(control_voltage,control_voltage);   //  1st is right hand wheel, swap motor uart port solve wheel speed diff problem，why??? 
   
  // use cmd below cannot make motor move, while set_motor_speed works, why???
  // use cmd below get left(serial1) speed 5, right(serial2) 18, same direction, cw. Serial1 -> L
  // Debug Process:
  // step 1, run-write motor driver on left motor, same
  // step 2, exchange left/right uart port, same
  // step 3, serial1 write -6, same
  //step 4, exchange  left/right power port,same
  // suspect left power line weak connection, change , same
    //  Serial1.print("T");
    //  Serial1.println(-6.0);
    //  Serial2.print("T");
    //  Serial2.println(8.0);


    // to be optimized to use a better  way waiting for data out instead of fix delay
    //remove comments below to enable progrma
    //  delay(100); 
    //  while (Serial1.available() == 0) {}
     
    //  while (Serial1.available() > 0 ) {
    //  float dummy_Byte = Serial1.read();
    //  }


    //  while (Serial2.available() == 0) {
    // //  Serial.print("Waiting for motor serial output...");
    //   }
     
    //  while (Serial2.available() > 0 ) {
    //  float dummy_Byte2 = Serial2.read();
    //  }
////////////////////////////////////////////


  

  /* just print to pc of what you just send on serial1/2 */
  // if (Serial1.available()) {
  //   int inByte = Serial1.read();
  //   Serial.write(inByte);
  // }
  // if (Serial2.available()) {
  //   int inByte = Serial2.read();
  //   Serial.write(inByte);
  // }

  // Manually set motor  Speed:
  //  Setup:
  //  1) Connect ESP Lionlite UART (TX1) to BLDC UART1(RX) 2) ESP Lionlite connect to PC
  // 2) Send motor cmd eg" T 10" to serial port 4)BLDC motor should move
  // read from PC port 0, send to BLDC Driver UART1, connect
  //  if (Serial.available()) {
  //    int inByte = Serial.read();
  //    Serial.println("Send ok !");
  //    Serial1.write(inByte);
  //   // }
}